/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/agvNavigate/profiler.cpp
 * @Description  : Profiler类用于性能分析和速度规划。
 *                 主要功能有创建Profiler对象、设置规划器、加载性能数据、生成速度限制值以及获取理论区域距离。
 *                 核心功能包括根据规划器状态生成速度值、处理补偿逻辑以及确保在特定条件下速度为零。 
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 18:05:57
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/
#include <stdlib.h>
#include <math.h> 
#include "profiler.h"
#include "tools.h"
#include "trajectory.h"
#include "z_types.h"
// #define REFRAIN_DOWN_SLOPE_K 2.0f	   // 抑制减速度速度系数
// #define DOWN_MODE_SUSPEND_SLOPE_K 2.5f // 挂起模式减速度系数
// #define RERAIN_MIN_VEL 0.2f			   // 抑制速度的最小速度
//////////////////////////////////////////////////////////////////////////////

/**
 * @brief 创建Profiler对象并初始化其属性
 *
 * 该函数用于创建Profiler对象，并初始化其各个属性。
 *
 * @param cruise_value 巡航值
 * @param refrain_value 抑制值
 * @param up_slope 上升斜率
 * @param dn_slope 下降斜率
 * @param up_small_slope 小幅度上升斜率
 * @param down_small_slope 小幅度下降斜率
 * @param tran_value 转换值
 * @param cycle 周期
 * @param type 类型
 * @param sure_area 确定区域
 * @param search0_value 搜索0值
 */
void Profiler::Create(float cruise_value, float refrain_value, float up_slope,
                      float dn_slope, float up_small_slope, float down_small_slope,
                      float tran_value, float cycle, short type, float sure_area,
                      float search0_value) {
  cruise_value_ = cruise_value;
  cycle_ = cycle;
  done_ = false;
  type_ = type;
  compensated_ = false;
  DEBUG_OUT(" profiler:Create: CruiseValue:" << cruise_value_ << ",nType:" << type);
}

/**
 * @brief 获取当前的规划模式
 *
 * 获取当前规划器的规划模式。如果规划器存在，则返回规划器的规划模式；否则返回默认的规划模式。
 *
 * @return 返回当前的规划模式，如果规划器为空则返回PROFILE_MODE_NORMAL。
 */
int Profiler::GetMode() const {
  if (planner_) {
    return planner_->plan_mode_.Mode();
  } else {
    DEBUG_ERROR_OUT("profiler:get mode failed; planner is null");
    return PROFILE_MODE_NORMAL;
  }
}

/**
 * @brief 加载性能分析数据
 *
 * 将性能分析所需的数据加载到 Profiler 对象中。
 * Set new current value, end value and area
 * @param cur_value 当前值
 * @param end_value 结束值
 * @param area 距离区域
 */
void Profiler::Load(float cur_value, float end_value, float area) {
  cur_value_ = cur_value;
  end_value_ = end_value;
  area_ = area;
  area_record_ = area;
  done_ = false;
}

/**
 * @brief 设置规划器
 *
 * 设置规划器，并返回一个布尔值表示是否成功。
 *
 * @param planner 要设置的规划器指针
 *
 * @return 如果成功设置规划器，则返回 true；否则返回 false
 */
bool Profiler::SetUpPlanner(OfflineScurvePlan* planner) {
  bool ret = false;
  if (planner) {
    planner_ = planner;
    ret = true;
    DEBUG_OUT("SetUpPlanner once");
  }
  return ret;
}


/**
 * @brief 根据给定的限制值生成速度值
 *
 * 根据当前规划器的状态和限制值，生成一个速度值。
 * By accelerating or decelerating to set m_fCurValue
 * Generate output automatically using internal data.
 * @param limit 限制值，用于限制生成的速度值
 * @return 返回生成的速度值
 */
float Profiler::LimitGenerate(float limit) {
  if (done_) return cur_value_;
  if (!planner_) {
    DEBUG_ERROR_OUT("error: planner is nullptr force set vel is 0");
    return 0.0f;
  }

  if (planner_->plan_mode_.Mode() == PROFILE_MODE_ESTOP) {
    cur_value_ = 0;
    if (area_ < 0) {
      done_ = true;
    }
    return cur_value_;
  }

  if (planner_->plan_mode_.ready_replan) {
    planner_->plan_mode_.ready_replan = false;
    planner_->target_info_.SetPlanFlag(true);
    DEBUG_OUT("ready_replan: set plan flag true once; cur_value_=" << cur_value_);
    if (cur_value_ < 0.0001) {
      cur_value_ = 0;
      DEBUG_OUT("readyReplan: set plan flag true once; clear curvalue 0.0f; m_fCurValue =" << cur_value_);
      return 0;
    }
  }

  if (planner_->plan_mode_.Mode() == PROFILE_MODE_REFRAIN) {
    if (planner_->target_info_.CheckRefrain(cur_value_)) {
      planner_->target_info_.SetPlanFlag(true);
      planner_->target_info_.SetRefrain(false, 0.4);
      DEBUG_OUT("ready_replan: refrain section2 set plan flag true once; cur_value_=" << cur_value_);
    }
  }

  double plan_rel_q = 0, plan_total_q = 0;
  planner_->GetPlanArea(plan_rel_q, plan_total_q);
  float theory_prog = plan_rel_q;
  float real_prog = area_record_ - area_;
  float delta_q = theory_prog - real_prog;
  DEBUG_OUT(" theoryProg=" << theory_prog << ", realProg=" << real_prog << ", deltaQ=" << delta_q);

  if (real_prog < 0) {
    DEBUG_WARN_OUT("warn: realProg < 0; realProg=" << real_prog);
    real_prog = 0;
  } else if (real_prog > area_record_) {
    DEBUG_WARN_OUT("warn: realProg > areaRecord; realProg=" << real_prog);
    real_prog = area_record_;
  }

  using PLAN_STEP = OfflineScurvePlan::PlanStep;
  cur_value_ = planner_->PlanVel();
  int step = planner_->GetStep();
  float v = 0.0;

  if (step > PLAN_STEP::Step2) {
    v = planner_->FindV(real_prog);
    DEBUG_OUT("find v=" << v << ", realProg=" << real_prog << ", old_cur_value_=" << cur_value_);
    cur_value_ = v;
  }

  if (area_ > 0.005f) {
    double plan_rel_q = 0, plan_total_q = 0;
    double extra_q = 0.00;
    planner_->GetPlanArea(plan_rel_q, plan_total_q);
    float remain_q = area_record_ - plan_rel_q;

    bool stop = fabsf(end_value_) < 0.0001;
    bool compensate = step == PLAN_STEP::Step7;

    if (compensate && stop) {
      if (!compensated_) {
        // Compensation logic can be added here if needed
        // compensated_ = true;
        // planner_->EndCompensation(area_ + extra_q);
        // DEBUG_OUT("EndCompensation: area_=" << area_ << ", extraQ=" << extra_q << ", set compensated true");
      }
      DEBUG_OUT("ready to compensate: "
                << "compensate=" << compensate
                << ", stop=" << stop
                << ", extraQ=" << extra_q
                << ", step=" << step
                << ", realArea=" << area_
                << ", theoryArea=" << remain_q
                << ", totalQ=" << plan_total_q);
    }

    if (type_ != (int)TrajType::SPIN_TRAJ) {
      bool canReplan = delta_q >= 0.13;
      bool valid = step >= PLAN_STEP::Step3;
      if (canReplan && valid) {
        DEBUG_OUT("canReplan line step:" << step
                  << ", cur_value_=" << cur_value_
                  << ", fTheoryRemainQ=" << remain_q
                  << ", realArea=" << area_);
        planner_->target_info_.SetPlanFlag(true);
      }
    }

    if (area_ <= 0.03f && fabs(end_value_) < 0.0001) {
      if (fabsf(cur_value_) < 0.003) {
        cur_value_ = 0.0f;
        done_ = true;
        DEBUG_OUT("#0 when area_ < 0.03f; (cur_value_ is < 0.003f) force set cur value 0.0 when end_value_ should be 0.0"
                  << ", area_=" << area_ << ", cur_value_=" << cur_value_);
        return cur_value_;
      }
    }
  } else if (fabsf(end_value_) < 0.0001) {
    if (type_ == (int)TrajType::SPIN_TRAJ) {
      cur_value_ = 0.0f;
      done_ = true;
      DEBUG_OUT("appEndValue is 0; spin finish");
      return cur_value_;
    }
    if (fabsf(cur_value_) < 0.005 && type_ != (int)TrajType::SPIN_TRAJ) {
      cur_value_ = 0.0f;
      done_ = true;
      DEBUG_OUT("#1 (cur_value_ is < 0.001f) force set cur value 0.0 when end_value_ should be 0.0"
                << ", area_=" << area_ << ", cur_value_=" << cur_value_);
      return cur_value_;
    } else if (area_ < 0.0001f) {
      cur_value_ = 0.0f;
      done_ = true;
      DEBUG_OUT("#2 (area_ < 0.000001f) force set cur value 0.0 when end_value_ should be 0.0"
                << ", area_=" << area_ << ", cur_value_=" << cur_value_);
      return cur_value_;
    }
  } else {
    done_ = true;
  }
  DEBUG_OUT("cur_value_ = " << cur_value_ << ", end_value_:" << end_value_
            << ", area_=" << area_);
  return cur_value_;
}

/**
 * @brief 获取规划理论区域距离
 *
 * 获取当前规划器所规划的理论区域距离。
 *
 * @return 返回剩余区域距离（以浮点数表示）。
 */
float Profiler::GetTheoryArea() {
  if (!planner_) {
    DEBUG_ERROR_OUT("GetTheoryArea: planner is nullptr");
    return 0.0f;
  }
  double plan_rel_q = 0, plan_total_q = 0;
  planner_->GetPlanArea(plan_rel_q, plan_total_q);
  float remain_q = area_record_ - plan_rel_q;
  return remain_q;
}